/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package FRC2856.Robot;
import edu.wpi.first.wpilibj.*;
import java.util.Stack;

/**
 *
 * @author sotille and magikarp-- and Yanz
 */
public class Autonomous {
    
    private Aiming eye;
    private Shooter shooter;
    private DriveTrain dt;
    private BridgeArm arm;
    private FileHandler fh;
    private int numLine;
    private int prevAutoCount;
    private int numDelay;
    private boolean success = false;
    private final boolean LOWERBRIDGE = false;
    public boolean autoDone = false;
    public int loopCount = 0;
    private final Capture cap;
    
    
    public Autonomous(Aiming a, Shooter f, DriveTrain dt, BridgeArm ba, Capture c)
    {
        //fh = new FileHandler();
        
        eye = a;
        shooter = f;
        this.dt = dt;
        arm= ba;
        numLine=0;
        prevAutoCount=0;
        numDelay=0;
        cap = c;
        //success = parseFile();
    }
    public void run ()
    {
        if(!autoDone){
            if (LOWERBRIDGE){
                if(!arm.swtiched())
                    arm.setMotion(0.2);
                else{
                    arm.setMotion(0);
                    autoDone = true;
                }
            }
            
            else{
                if(loopCount == 0)
                    shooter.spinRateUpdate(42);
                if(loopCount == 80)
                    cap.setUpperSpeed(1);
                if(loopCount == 580){
                    cap.setUpperSpeed(0);
                    shooter.spinEffort(0);
                    autoDone = true;
                    shooter.spinStop();
                }
                loopCount++;
            }
        }
    }
    public boolean parseFile()
    {
        return parseFile(false);
    }
    public boolean parseFile(boolean exec)
    {
        try
        {
            String file = fh.readFile();
        if (file == null)
        {
            System.out.println("auto.txt read failed");
            return false;
        }
        String[] lines = Utils.split(file, "\n");//split the files at the lines, get an array containing each line
        for (int i = 0; i < lines.length; i++)
        {
            //get rid of excess white space
            String line = lines[i].trim();
            
            //lines that start with (or contain really) # are comments
            if (line.indexOf("#") != -1)
                continue;
            
            if (line.indexOf("moveEffort") != -1)
            {
                String[] params = getParams(line);
                double a = Double.parseDouble(params[0]);
                double b = Double.parseDouble(params[1]);
                //only when parse is called with true it actually runs the commands
                //otherwise all the parsing is done to see if we can parse successfully
                if (exec)
                    dt.moveEffort(a,b);
            }
            if (line.indexOf("moveDist") != -1)
            {
                String[] params = getParams(line);
                double a = Double.parseDouble(params[0]);
                double b = Double.parseDouble(params[1]);
                if (exec)
                    dt.moveDist(a, b);
            }
            if (line.indexOf("aim") != -1)
            {
                String[] params = getParams(line);
                int a = Integer.parseInt(params[0]);
                if (exec)
                    eye.aim(a);
            }
            if (line.indexOf("aimAndShoot") != -1)//if you tell it to aim and shoot
            {
                String[] params = getParams(line);                
                int a = Integer.parseInt(params[0]);
                if (exec)
                {
                    eye.aim(a);
                    //shooter.Fire();  // FixMe
                }
            }
            if (line.indexOf("lowerBridgeArm") != -1)//if you tell it to lower the bridge arm
            {
                if (exec)
                {
                    arm.setMotion(-0.5);
                    Timer.delay(3);
                    arm.setMotion(0);
                }
            } 
            if (line.indexOf("raiseBridgeArm") != -1)//if you tell it to raise the bridge arm
            {
                if (exec)
                {
                    arm.setMotion(0.5);
                    Timer.delay(3);
                    arm.setMotion(0);
                }
            } 
            if (line.indexOf("fire") != -1)
            {
                if (exec){
                     //shooter.Fire();  // FixMe
                }
            } 
            if (line.indexOf("delay") != -1)
            {
                String[] params = getParams(line);
                double a = Double.parseDouble(params[0]);
                if (exec)
                    Timer.delay(a);
                
            }
        }   
        return true;
        }
        catch(NumberFormatException e)
        {
            System.out.println("Parameter is not correct " + e.getMessage());
            return false;
        }
        catch(Exception e)
        {
            System.out.println("General parse error " + e.getMessage());
            return false;
        }
    }
   
    
    //this aims at the top basket and shoots without moving at all.
    //Good if we cancalibrate the speed based on the distance form the basket to the key.
    
    public void standAndShoot(){
        
       eye.aim(3);
       //shooter.Fire();  // FixMe
          
    }
    
    public void driveAndShoot(){
        
        dt.moveEffort(.25, .25);//go up to the basket
        Timer.delay(3);//we need to test these numbers
        
        eye.aim(0);//aim and shoot lower basket
        //shooter.Fire();  // FixMe
        
    }
    
    //Int corresponds to basket you want to aim at 0,1,2, or 3
    public void pickABasket(int i){
        eye.aim(i);
        //shooter.Fire();  // FixMe
    }
    public String[] getParams (String line)
    {
        String temp = line.substring(line.indexOf("(")+1, line.indexOf(")"));
        return Utils.split(temp, ",");
    }
}
